#!/usr/bin/env python
#  -*-coding:utf8 -*-
 
from ast import Str
import string
import rospy
import numpy as np
from std_msgs.msg import Int64
#import message_filters
from hellobot_msgs.msg import msg_battery, msg_call, msg_pose
from std_msgs.msg import String

BatteryCharging = False
TargetPos = msg_call()

def MakeMovement():
    #建立运动指令
    x = 2
    return x

class Main():
    def __init__(self):
        """ t1 = message_filters.Subscriber('Calling', Float64MultiArray)
        t2 = message_filters.Subscriber('Battery', String)
        
        ts = message_filters.ApproximateTimeSynchronizer([t1, t2], 10, 1, allow_headerless=True)
        ts.registerCallback(self.callback) """
        
        rospy.Subscriber('Calling', msg_call, self.callbackCalling)
        rospy.Subscriber('Battery', msg_battery, self.callbackBattery)
        rospy.Subscriber('Pose', msg_pose, self.callbackPose)
        rospy.Subscriber('Command', String, self.callbackCommand)

        self.pub = rospy.Publisher('Move', msg_call, queue_size=1)
        rate = rospy.Rate(60)
    
    def callbackCalling(self, data):
        # if data.newstatus == False:
        #     print('waiting for calling')
        #     #do some sparetime moves?
        # else:
        #     #function to deliver a movement
        #     if BatteryCharging == False:
        #         global TargetPos
        #         TargetPos = data
        #         self.pub.publish(data)
        #         msg = 'target is : x-'+ str(int(data.x)) + ' y-' + str(int(data.y)) + ' z-' + str(int(data.z)) + ';'
        #         rospy.loginfo(msg)
        global TargetPos
        TargetPos = data
        self.pub.publish(data)
        #msg = 'status is ' + str(data.newstatus) + ';  target is : x-'+ str(int(data.x)) + ' y-' + str(int(data.y)) + ' z-' + str(int(data.z)) + ';'
        #rospy.loginfo(msg)
        rospy.loginfo("status: %1.0f ; the target: %3.4f %3.4f %3.4f\n",data.newstatus,data.x,data.y,data.z)

    def callbackBattery(self, data):
        global BatteryCharging
        rospy.loginfo("remaining: %3.1f%%\n",data.remain)
        #print('remaining: {:.1f}%'.format(data.remain))
        if data.status == False:
            BatteryCharging = False
            print('waiting for orders')
        else:
            BatteryCharging = True
            print('battery is in charging')

    def callbackPose(self, data):
        # if BatteryCharging == False:
        #     if data.newstatus == True:
        #         TargetPos = data
        #         self.pub.publish(data)
        rospy.loginfo("the pose is: %3.4f %3.4f %3.4f %3.4f %3.4f %3.4f\n",data.x,data.y,data.z,data.alpha,data.beta,data.gamma)

    def callbackCommand(self, data):
        #some commands from client
        msg = 'i hear a command from client: '+ data.data
        rospy.loginfo(msg)


if __name__ == '__main__':
    rospy.init_node('Main')
    try:
        Main()
    except rospy.ROSInterruptException:
        pass
    rospy.spin()
